#include "Components/clock.h"
#include "Components/motor.h"
#include "Components/oled_hardware_i2c.h"
#include "Components/uart.h"
#include "ti_msp_dl_config.h"
#include <ti/driverlib/m0p/dl_interrupt.h>
#include "motion.h"
#include "line_patrol.h"
#include "servo.h"

#define TAG "Main"

char _str_all[128] = {0};
#define my_printf(str, ...)              \
  sprintf(_str_all, str, ##__VA_ARGS__); \
  uart0_send_string(_str_all);

#define abs(x) ((x) > 0 ? (x) : -(x))

// PI控制器结构体 (int16版本)
typedef struct
{
  int16_t Kp;         // 比例增益 (Q8.8格式)
  int16_t Ki;         // 积分增益 (Q8.8格式)
  int16_t dt;         // 控制周期 (ms)
  int16_t max_output; // 输出限幅
  int16_t integral;   // 积分累积值 (Q8.8格式)
  int16_t prev_error; // 前次误差 (Q8.8格式)
  int16_t scale;      // 数值缩放因子
} Int16PIController;

// 初始化PI控制器
void PI16_Init(Int16PIController *pi,
               int16_t Kp_int, int16_t Ki_int,
               int16_t dt_ms, int16_t max_output,
               int16_t scale)
{
  // 转换为Q8.8格式 (256 = 1.0)
  pi->Kp = Kp_int * 256 / scale;
  pi->Ki = Ki_int * 256 / scale;
  pi->dt = dt_ms;
  pi->max_output = max_output;
  pi->integral = 0;
  pi->prev_error = 0;
  pi->scale = scale;
}

// 更新PI控制器状态并计算输出 (纯int16运算)
int16_t PI16_Update(Int16PIController *pi, int16_t error)
{
  // 转换为Q8.8格式
  int16_t error_fixed = error * 256 / pi->scale;

  // 计算误差平均值 (梯形积分)
  int16_t error_avg = (error_fixed + pi->prev_error) / 2;

  // 积分项计算: I = Ki * ∫e dt
  int16_t d_integral = (pi->Ki * pi->dt * error_avg) / 1000;
  pi->integral += d_integral;

  // 比例项计算: P = Kp * e
  int16_t P_term = (pi->Kp * error_fixed) / 256;

  // 积分项输出: I = Ki * integral
  int16_t I_term = pi->integral / 256; // 从Q8.8转回整数

  // PI输出计算
  int16_t output = P_term + I_term;

  // 更新前次误差
  pi->prev_error = error_fixed;

  // 输出限幅和抗积分饱和
  if (output > pi->max_output)
  {
    output = pi->max_output;
    pi->integral -= d_integral; // 回退积分
  }
  else if (output < -pi->max_output)
  {
    output = -pi->max_output;
    pi->integral -= d_integral; // 回退积分
  }

  return output;
}
int _main()
{
  // 测试参数 (使用毫米单位)
  const int16_t target_radius = 5000; // 5米 = 5000毫米
  int16_t actual_radius = 30000;      // 10米 = 10000毫米
  const int16_t dt_ms = 100;          // 控制周期100ms
  const int16_t total_time = 10000;   // 总模拟时间10秒
  const int16_t steps = total_time / dt_ms;

  // 初始化PI控制器
  Int16PIController pi;
  // 参数: Kp=0.8, Ki=0.2, 输出限幅=500, 缩放因子=1000
  PI16_Init(&pi, 3000, 2500, dt_ms, 5000, 1000);

  my_printf("int16 PI Controller for Turning Radius\n");
  my_printf("Target Radius: %d mm\n", target_radius);
  my_printf("Initial Radius: %d mm\n", actual_radius);
  my_printf("Control Parameters: Kp=0.8, Ki=0.2, dt=%d ms\n\n", dt_ms);

  // 模拟控制循环
  for (int16_t i = 0; i < steps; i++)
  {
    int16_t error = target_radius - actual_radius;
    int16_t control = PI16_Update(&pi, error);

    // 简化系统模型: 实际半径变化
    actual_radius += control / 10; // 控制输出转换为毫米变化量

    // 每秒打印一次状态
    if (i % 10 == 0)
    {
      my_printf("Time: %d ms | Radius: %d mm | Error: %d mm | Control: %d\n",
             i * dt_ms, actual_radius, error, control);
    }
  }

  // 最终结果
  int16_t final_error = target_radius - actual_radius;
  my_printf("\nFinal Results:\n");
  my_printf("Target Radius: %d mm\n", target_radius);
  my_printf("Actual Radius: %d mm\n", actual_radius);
  my_printf("Final Error: %d mm\n", final_error);
  my_printf("Steady-state: %s\n", abs(final_error) < 50 ? "Yes" : "No");

  return 0;
}

int main(void)
{
  SYSCFG_DL_init();
  SysTick_Init();

  uart_init();

  uart0_Log_str(TAG, "Init sucessfully!");

  _main();

  while (1)
  {
    mspm0_delay_ms(1000);
  }
}
